
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       lpfilter.c
  * @author     baiyang
  * @date       2021-7-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "lpfilter.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/** 
  * @brief       一阶低通滤波
  * @param[in]   pLPFObject  
  * @param[in]   in    
  * @param[out]  
  * @retval      
  * @note        参考函数参考ardupilot的滤波库 /libraies/Filter/LowPassFilter
  */
float lpf_apply1(LowPassFilt* pLPFObject, float in)
{
    //判断参数合理性
    if (pLPFObject->cutoff_freq <= 0.0f || pLPFObject->alpha <= 0.0f)
    {
        pLPFObject->output = in;
        return pLPFObject->output;
    }

    pLPFObject->output += (in - pLPFObject->output) * pLPFObject->alpha;

    return pLPFObject->output;
}

/** 
  * @brief       一阶低通滤波
  * @param[in]   pLPFObject  
  * @param[in]   in  
  * @param[in]   dt  
  * @param[out]  
  * @retval      
  * @note        
  */
float lpf_apply2(LowPassFilt* pLPFObject, float in, float dt)
{
    //判断参数合理性
    if (pLPFObject->cutoff_freq <= 0.0f || dt <= 0.0f)
    {
        pLPFObject->output = in;
        return pLPFObject->output;
    }

    float rc = 1.0f/(M_2PI * pLPFObject->cutoff_freq);
    //输入系数
    float alpha = dt / (dt + rc);
    pLPFObject->output += (in - pLPFObject->output) * alpha;

    return pLPFObject->output;
}

Vector2f_t lpf_apply1_vec2f(LowPassFilt_vec2f* pLPFObject, const Vector2f_t* in)
{
    if (pLPFObject->cutoff_freq <= 0.0f || pLPFObject->alpha <= 0.0f)
    {
        pLPFObject->output = *in;
        return pLPFObject->output;
    }

    pLPFObject->output.x += (in->x - pLPFObject->output.x)*pLPFObject->alpha;
    pLPFObject->output.y += (in->y - pLPFObject->output.y)*pLPFObject->alpha;

    return pLPFObject->output;
}

/** 
  * @brief       一阶低通滤波
  * @param[in]   pLPFObject  
  * @param[in]   in  
  * @param[in]   dt  
  * @param[out]  
  * @retval      
  * @note        参考函数参考ardupilot的滤波库 /libraies/Filter/LowPassFilter
  */
Vector2f_t lpf_apply2_vec2f(LowPassFilt_vec2f* pLPFObject, const Vector2f_t* in, float dt)
{
    if (pLPFObject->cutoff_freq <= 0.0f || dt <= 0.0f)
    {
        pLPFObject->output = *in;
        return pLPFObject->output;
    }

    float rc = 1 / (2 * M_PI * pLPFObject->cutoff_freq);
    //输入系数
    float alpha = dt/(dt + rc);

    pLPFObject->output.x += (in->x - pLPFObject->output.x)*alpha;
    pLPFObject->output.y += (in->y - pLPFObject->output.y)*alpha;
    return pLPFObject->output;
}

Vector3f_t lpf_apply1_vec3f(LowPassFilt_vec3f* pLPFObject, const Vector3f_t* in)
{
    if (pLPFObject->cutoff_freq <= 0.0f || pLPFObject->alpha <= 0.0f)
    {
        pLPFObject->output = *in;
        return pLPFObject->output;
    }

    pLPFObject->output.x += (in->x - pLPFObject->output.x)*pLPFObject->alpha;
    pLPFObject->output.y += (in->y - pLPFObject->output.y)*pLPFObject->alpha;
    pLPFObject->output.z += (in->z - pLPFObject->output.z)*pLPFObject->alpha;

    return pLPFObject->output;
}

/** 
  * @brief       一阶低通滤波
  * @param[in]   pLPFObject  
  * @param[in]   in  
  * @param[in]   dt  
  * @param[out]  
  * @retval      
  * @note        参考函数参考ardupilot的滤波库 /libraies/Filter/LowPassFilter根据需求改变
*/
Vector3f_t lpf_apply2_vec3f(LowPassFilt_vec3f* pLPFObject, const Vector3f_t* in, float dt)
{
    if (pLPFObject->cutoff_freq <= 0.0f || dt <= 0.0f)
    {
        pLPFObject->output = *in;
        return pLPFObject->output;
    }

    float rc = 1 / (2 * M_PI * pLPFObject->cutoff_freq);
    //输入系数
    float alpha = dt/(dt + rc);

    pLPFObject->output.x += (in->x - pLPFObject->output.x)*alpha;
    pLPFObject->output.y += (in->y - pLPFObject->output.y)*alpha;
    pLPFObject->output.z += (in->z - pLPFObject->output.z)*alpha;

    return pLPFObject->output;
}

void lpf_set_cutoff1(LowPassFilt* pLPFObject, float cutoff_freq)
{
    pLPFObject->cutoff_freq = cutoff_freq;
}

void lpf_set_cutoff2(LowPassFilt* pLPFObject, float cutoff_freq, float dt)
{
    pLPFObject->cutoff_freq = cutoff_freq;

    pLPFObject->alpha = math_calc_lpf_alpha_dt(dt,cutoff_freq);
}

void lpf_set_cutoff1_vec2f(LowPassFilt_vec2f* pLPFObject, float cutoff_freq)
{
    pLPFObject->cutoff_freq = cutoff_freq;
}

void lpf_set_cutoff2_vec2f(LowPassFilt_vec2f* pLPFObject, float cutoff_freq, float dt)
{
    pLPFObject->cutoff_freq = cutoff_freq;

    pLPFObject->alpha = math_calc_lpf_alpha_dt(dt,cutoff_freq);
}

void lpf_set_cutoff1_vec3f(LowPassFilt_vec3f* pLPFObject, float cutoff_freq)
{
    pLPFObject->cutoff_freq = cutoff_freq;
}

void lpf_set_cutoff2_vec3f(LowPassFilt_vec3f* pLPFObject, float cutoff_freq, float dt)
{
    pLPFObject->cutoff_freq = cutoff_freq;

    pLPFObject->alpha = math_calc_lpf_alpha_dt(dt,cutoff_freq);
}

/*------------------------------------test------------------------------------*/


